within ThreeD_MBS_Dynamics.Examples.Snakeboard;

model IdealModel
  parameter SI.Mass m = 6;
  parameter SI.Length l = 0.2;
  parameter SI.MomentOfInertia J = 0.016;
  parameter SI.MomentOfInertia Jr = 0.072;
  parameter SI.MomentOfInertia Jw = 0.0013;
  parameter SI.Angle a_psi = 0.7;
  parameter SI.Angle beta_psi = 0;
  parameter SI.AngularVelocity omega_psi = 3;
  parameter SI.Angle a_f = 1;
  parameter SI.Angle beta_f = 0;
  parameter SI.AngularVelocity omega_f = 2;
  parameter SI.Angle a_b = -1;
  parameter SI.Angle beta_b = 0;
  parameter SI.AngularVelocity omega_b = 2;
  SI.Angle psi;
  SI.Angle phi_b;
  SI.Angle phi_f;
  SI.Angle theta;
  SI.AngularVelocity dtheta;
  SI.AngularVelocity dphi_b(start = a_b*omega_b*cos(beta_b));
  SI.AngularVelocity dphi_f(start = a_f*omega_f*cos(beta_f));
  SI.AngularVelocity dphi_fa;
  SI.AngularVelocity dpsi(start = a_psi*omega_psi*cos(beta_psi));
  SI.AngularVelocity dpsi_a;
  SI.AngularAcceleration ddtheta;
  SI.AngularAcceleration ddphi_b;
  SI.AngularAcceleration ddphi_f;
  SI.AngularAcceleration ddphi_fa;
  SI.AngularAcceleration ddpsi;
  SI.AngularAcceleration ddpsi_a;
  SI.Torque u_psi;
  SI.Torque u_f;
  SI.Torque u_b;
  SI.Torque M;
  SI.Torque BoardM;
  SI.Position x;
  SI.Position y;
  SI.Position z;
  SI.Velocity vx;
  SI.Velocity vy;
  SI.Acceleration ax;
  SI.Acceleration ay;
  SI.Force lambda_f;
  SI.Force lambda_b;
  SI.Force X;
  SI.Force Y;
  SI.Force F_fn;
  SI.Force F_bn;
equation
  z = -y;
  der(psi) = dpsi;
  der(phi_f) = dphi_f;
  der(phi_b) = dphi_b;
  der(theta) = dtheta;
  der(dpsi) = ddpsi;
  der(dphi_f) = ddphi_f;
  der(dphi_b) = ddphi_b;
  der(dtheta) = ddtheta;
  der(x) = vx;
  der(y) = vy;
  der(vx) = ax;
  der(vy) = ay;
//  m*ax - lambda_b*sin(phi_b + theta) - lambda_f*sin(phi_f + theta) = cos(theta);
//  m*ay + lambda_b*cos(phi_b + theta) + lambda_f*cos(phi_f + theta) = sin(theta);
  m*ax - lambda_b*sin(phi_b + theta) - lambda_f*sin(phi_f + theta) = 0;
  m*ay + lambda_b*cos(phi_b + theta) + lambda_f*cos(phi_f + theta) = 0;
  (J + Jr + 2*Jw)*ddtheta + Jr*ddpsi + Jw*(ddphi_f + ddphi_b) - lambda_b*l*cos(phi_b) + lambda_f*l*cos(phi_f) = 0;
  Jr*(ddpsi + ddtheta) = u_psi;
  Jw*(ddphi_f + ddtheta) = u_f;
  Jw*(ddphi_b + ddtheta) = u_b;
  -sin(phi_b + theta)*vx + cos(phi_b + theta)*vy - l*cos(phi_b)*dtheta = 0;
  -sin(phi_f + theta)*vx + cos(phi_f + theta)*vy + l*cos(phi_f)*dtheta = 0;
  ddpsi = -a_psi*omega_psi^2*sin(omega_psi*time + beta_psi);
//  ddphi_f = 0;
  ddphi_f = -a_f*omega_f^2*sin(omega_f*time + beta_f);
  ddphi_b = -a_b*omega_b^2*sin(omega_b*time + beta_b);
  dphi_fa = dtheta + dphi_f;
  ddphi_fa = ddtheta + ddphi_f;
  dpsi_a = dtheta + dpsi;
  ddpsi_a = ddtheta + ddpsi;
  M = lambda_b*l*cos(phi_b) - lambda_f*l*cos(phi_f);
  BoardM = J*ddtheta;
  X = m*ax;
  Y = m*ay;
  F_bn = lambda_b;
  F_fn = lambda_f;
  annotation (
    Diagram,
    experiment(
      StopTime=25,
      NumberOfIntervals=8000,
      Tolerance=1e-012),
    experimentSetupOutput,
    DymolaStoredErrors);
end IdealModel;
